/*----------------------------------------------------------------------------*/
/* Team 3647
/* DriveBase.java
/* Has all driving functions and bridge lowering
/*----------------------------------------------------------------------------*/
package Kit;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.KinectStick;

public class DriveBase {

    public static Joystick Stick1 = new Joystick(1);
    static KinectStick leftArm = new KinectStick(1);
    static KinectStick rightArm = new KinectStick(2);
    //Victors
    static LinearVictor VictorRight1 = new LinearVictor(1);
    static LinearVictor VictorRight2 = new LinearVictor(2);
    static LinearVictor VictorLeft1 = new LinearVictor(4);
    static LinearVictor VictorLeft2 = new LinearVictor(5);

    public static void KinectDrive() {
        double LeftJoyStick  = .93*leftArm.getY();
        double RightJoyStick = .93*rightArm.getY();

        double throttle = (LeftJoyStick + RightJoyStick) / 2;
        double wheel = (LeftJoyStick - RightJoyStick) / 2;

        if (Math.abs(throttle) <= 0.3 && Math.abs(throttle) >= 0.05) {
            throttle = 0.3 * (throttle < 0 ? -1 : 1);
        } else if (Math.abs(throttle) < 0.05) {
            throttle = 0;
        }
        CheesyDrive(throttle, wheel, true);
    }

    /***************************************************************************
     * Drive function based off joy stick commands 
     * Uses the drive function.
     ***************************************************************************/
    public static void TankCD() {
        double LeftJoyStick = Stick1.getRawAxis(2);
        double RightJoyStick = Stick1.getRawAxis(4);

        double throttle = (LeftJoyStick + RightJoyStick) / 2;
        double wheel = (LeftJoyStick - RightJoyStick) / 2;

        if (Math.abs(throttle) <= 0.3 && Math.abs(throttle) >= 0.05) {
            throttle = 0.3 * (throttle < 0 ? -1 : 1);
        } else if (Math.abs(throttle) < 0.05) {
            throttle = 0;
        }

        CheesyDrive(throttle, wheel, true);
    }

    public static void CheesyDrive(double throttle, double wheel, boolean quickTurn) {
        double angular_power = 0.0;
        double overPower = 0.0;
        double sensitivity = 1.5;
        double rPower = 0.0;
        double lPower = 0.0;

        if (quickTurn) {
            overPower = 1.0;
            sensitivity = 1.0;
            angular_power = wheel;
        } else {
            overPower = 0.0;
            angular_power = Math.abs(throttle) * wheel * sensitivity;
        }
        rPower = lPower = throttle;
        lPower += angular_power;
        rPower -= angular_power;

        if (lPower > 1.0) {
            rPower -= overPower * (lPower - 1.0);
            lPower = 1.0;
        } else if (rPower > 1.0) {
            lPower -= overPower * (rPower - 1.0);
            rPower = 1.0;
        } else if (lPower < -1.0) {
            rPower += overPower * (-1.0 - lPower);
            lPower = -1.0;
        } else if (rPower < -1.0) {
            lPower += overPower * (-1.0 - rPower);
            rPower = -1.0;
        }
        Drive(lPower, rPower);
    }

    /***************************************************************************
     * Drive function
     * Left and Right
     ***************************************************************************/
    public static void Drive(double left, double right) {
        VictorLeft1.set(left);
        VictorLeft2.set(left);

        VictorRight1.set(-right);
        VictorRight2.set(-right);
    }
}
